///////////////////////////////////////////////////////////////////////
//
// NAME:		pid.c
//
// AUTHOR:	Tom Bottiglieri
//
// DATE:		12/18/07
//
// License:		GPL v3.0 (http://www.gnu.org/copyleft/gpl.html)
//
// PID Driver (see pid_readme.txt)
//
///////////////////////////////////////////////////////////////////////
#include "pid.h"

char getPID(PID * in, int set_point, int position){
	long pTerm, dTerm, iTerm, error, output;	
	error = set_point - position;

	 // calculate the proportional term
	pTerm = (long)(in->pGain * error)/ in->div;  

	// calculate the integral state with appropriate limiting
	in->iState += error;

	//limit I
	if (in->iState > in->iMax)
		in->iState = in->iMax;
	else if (in->iState < -in->iMax)
		in->iState = -in->iMax;

	// calculate the integral term
	iTerm = (in->iGain * in->iState)/ in->div;  

	//calculate d term
	dTerm = (in->dGain * (error - in->dState) ) / in->div;
	in->dState = error;

	output= pTerm + iTerm + dTerm;

	if(output>127) output=127;
	else if(output < -127) output=-127;

	return (char) output;
}


void newPID(PID *in,int p, int i, int d, int i_max, int divisor){
    in->pGain = p;
    in->iGain = i;
    in->dGain = d;
	in->iMax=i_max;
	in->div=divisor;
}
